#include "ros/ros.h"
#include "std_msgs/String.h"
// #include "sensor_msgs/LaserScan.h"
#include <vector>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp> 
#include <opencv2/imgproc/imgproc.hpp>  
#include "iostream"
#include <typeinfo>
#include <sstream>
#include <string>
#include <fstream>

using namespace cv;

using namespace std;

int record_begin_num;

// sensor_msgs::LaserScan lidarData;
cv_bridge::CvImagePtr cv_ptr;

// void rplidarData_subCallback(const sensor_msgs::LaserScan::ConstPtr& lddat)
// {
// 	lidarData = *lddat;
// }

void image_Callback(const sensor_msgs::ImageConstPtr& msg){
	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
	}
}
 
string getPicFileName(string &basePath, int count){
    string res;
    stringstream sstr;
    sstr << count;
    string tempdst;
    sstr >> tempdst;

    res = basePath + tempdst + ".jpg";

    return res;
}

void txtDataRecord(string &txtPath, int count, int path, float numf_min, int index_min){
    ofstream fout(txtPath.c_str(), ios::app);

	fout << count << " : " << path << " " << numf_min << " " << index_min << endl;
    
	fout.close();
}

int getPath(vector<float> &lidar, int start, int end, float &minValue, int &minVarIndex){
    int res = 0;
    float LENGTH_THRESHOLD = 2.0;
    int INDEX_NUM_THRESHOLD = 1;
    vector<vector<int> > temp2d;
    vector<int> temp1d;
    int maxSize = 0;
    int maxIndex = 0;
    int cnt = 0;

	float minVar = 12.0;
	int minIndex = 0;

    for(int i = start; i <= end; ++i){
		if(lidar[i] < minVar){
			minVar = lidar[i];
			minIndex = i;
		}

        if(lidar[i] >= LENGTH_THRESHOLD){
            temp1d.push_back(i);
            if(i == end){
                if(!temp1d.empty()){
                    if(temp1d.size() > maxSize){
                        maxSize = (int)temp1d.size();
                        maxIndex = cnt;
                    }
                    temp2d.push_back(temp1d);
                    temp1d.clear();
                    cnt++;
                }
            }
        }
        else{
            if(!temp1d.empty()){
                if(temp1d.size() > maxSize){
                    maxSize = (int)temp1d.size();
                    maxIndex = cnt;
                }
                temp2d.push_back(temp1d);
                temp1d.clear();
                cnt++;
            }
        }

    }

	minValue = minVar;
	minVarIndex = minIndex - 180;

    if(!temp2d.empty() && temp2d[maxIndex].size() >= INDEX_NUM_THRESHOLD){
        res = temp2d[maxIndex][0] + maxSize / 2 - 180;
    }

    return res;
}

int main(int argc, char *argv[])
{

	ros::init(argc, argv, "pic_cap");
	ros::NodeHandle n;

	// ros::Subscriber lidar_sub = n.subscribe("/scan", 1, rplidarData_subCallback);
	ros::Subscriber image_sub_ = n.subscribe("/usb_cam/image_raw", 1, image_Callback);
	
	n.param<int>("record_begin_num", record_begin_num, 0);

	ros::Rate rate(10);
	

	// vector<float> lidar_data;

	int RECORD_MAX_NUM = 100000;
	int BEGIN_NUM = 0; 
	int count = BEGIN_NUM;
	int RECORD_LIMIT_NUM = BEGIN_NUM + RECORD_MAX_NUM;
	string basePath = "../../../../realDatasetPath_test/RGBpic/";
	string txtPath = "../../../../realDatasetPath_test/data_json/data.txt";

	int minValueIndexMain = 0;
	int pathIndex = 0;
    ROS_INFO("picture capture begin");
	while(ros::ok()){
		ros::spinOnce();
		rate.sleep();

		if(cv_ptr){
			cv::Mat img = cv_ptr->image;

			// imshow("pic",img);
        	cv::waitKey(3); 

			cv::imwrite(getPicFileName(basePath, count), img); //保存处理后的图片

			count++;
			//cout << count << endl;

			if(count % 100 == 0){
				cout << "record the " << count << "th data..." << endl; 
			}

			if(count >= RECORD_LIMIT_NUM){
				printf("record finished!!!\n");
				return 0;
			}
		}

	}

	return 0;
}
